copp\copp\copp2/
interpolation.rs

1//! Interpolation and profile-conversion utilities for second-order path parameterization.
2//!
3//! # Method identity
4//! This module serves both:
5//! - **Time-Optimal Path Parameterization (TOPP2)** workflows,
6//! - **Convex-Objective Path Parameterization (COPP2)** workflows.
7//!
8//! # Scope
9//! This module provides deterministic conversions among:
10//! - path-parameter profile `a(s) = \dot{s}^2`,
11//! - derivative-like profile `b(s) = \frac{1}{2}\frac{da}{ds}` on segments,
12//! - time mapping `t(s)` and inverse sampling `s(t)`.
13//!
14//! # Conventions
15//! - Path grid uses station samples `s[0..=n]`.
16//! - State profile `a` is node-based (`a.len() == s.len()`).
17//! - Profile `b` is segment-based (`b.len() == s.len() - 1`).
18
19use crate::copp::InterpolationMode;
20use itertools::izip;
21
22/// Compute segment profile `b` from node profile `a`.
23///
24/// # Definition
25/// For each segment `[s_k, s_{k+1}]`, this function computes:
26/// $b_k = \frac{1}{2}\frac{a_{k+1}-a_k}{s_{k+1}-s_k}$.
27///
28/// # Input contract
29/// - valid when `s.len() >= 2` and `a.len() == s.len()`;
30/// - otherwise returns an empty vector.
31///
32/// # Returns
33/// Returns `b` with `b.len() == s.len() - 1`.
34///
35/// # Errors
36/// This function does not return `Result`; invalid input is mapped to empty output.
37///
38/// # Contract
39/// - Output ordering is consistent with segment ordering on `s.windows(2)`.
40/// - No allocation beyond returned vector and iterator temporaries.
41pub fn a_to_b_topp2(s: &[f64], a: &[f64]) -> Vec<f64> {
42    if s.len() < 2 || a.len() != s.len() {
43        return vec![];
44    }
45    s.windows(2)
46        .zip(a.windows(2))
47        .map(|(s_pair, a_pair)| 0.5 * (a_pair[1] - a_pair[0]) / (s_pair[1] - s_pair[0]))
48        .collect::<Vec<f64>>()
49}
50
51/// Compute cumulative time profile `t(s)` from `a(s)`.
52///
53/// # Semantics
54/// - `t_s[i]` is the time at station `s[i]`.
55/// - initial condition is `t_s[0] = t0`.
56/// - returns `(t_final, t_s)` where `t_final == *t_s.last().unwrap()`.
57///
58/// # Input contract
59/// - valid when `s.len() >= 2` and `a.len() == s.len()`;
60/// - otherwise returns `(NaN, empty)`.
61///
62/// # Returns
63/// Returns `(t_final, t_s)` with `t_s.len() == s.len()` on valid input.
64///
65/// # Errors
66/// This function does not return `Result`; invalid input is mapped to `(NaN, vec![])`.
67///
68/// # Contract
69/// - `t_s` is monotonically increasing when `a` is nonnegative and `s` is increasing.
70/// - `t_s[0] == t0` always holds on valid input.
71pub fn s_to_t_topp2(s: &[f64], a: &[f64], t0: f64) -> (f64, Vec<f64>) {
72    if s.len() < 2 || a.len() != s.len() {
73        return (f64::NAN, vec![]);
74    }
75    // Map s to t
76    let mut t_s = Vec::<f64>::with_capacity(s.len()); // t_s[i] = t(s[i]), begin from t0
77    let mut t_prev = t0;
78    t_s.push(t_prev);
79    for (s_pair, a_pair) in s.windows(2).zip(a.windows(2)) {
80        t_prev += 2.0 * (s_pair[1] - s_pair[0]) / (a_pair[0].sqrt() + a_pair[1].sqrt());
81        t_s.push(t_prev);
82    }
83    (t_prev, t_s)
84}
85
86/// Interpolate inverse mapping `s(t)` from `a(s)` and sampled `t(s)`.
87///
88/// # Modes
89/// - `UniformTimeGrid(t0, dt, include_final)`: generate uniform time samples;
90/// - `NonUniformTimeGrid(t_sample)`: use caller-provided increasing samples.
91///
92/// # Input contract
93/// - requires `s.len() >= 2`, `a.len() == s.len()`, `t_s.len() == s.len()`;
94/// - requires `t_s` strictly increasing.
95/// - invalid input returns empty vector.
96///
97/// # Output semantics
98/// - output length matches requested sample count in each mode;
99/// - for out-of-range time samples, output entries are `NaN`.
100///
101/// # Returns
102/// Returns sampled `s(t)` values according to `mode`.
103///
104/// # Errors
105/// This function does not return `Result`; invalid input or invalid `mode` settings
106/// are mapped to empty output.
107///
108/// # Contract
109/// - preserves requested sample order;
110/// - never panics for malformed user input (falls back to empty vector).
111pub fn t_to_s_topp2(s: &[f64], a: &[f64], t_s: &[f64], mode: InterpolationMode<'_>) -> Vec<f64> {
112    if s.len() < 2
113        || a.len() != s.len()
114        || t_s.len() != s.len()
115        || t_s.windows(2).any(|w| w[0] >= w[1])
116    {
117        return vec![];
118    }
119    match mode {
120        InterpolationMode::UniformTimeGrid(t0, dt, include_final) => {
121            if dt <= 0.0 {
122                return vec![];
123            }
124            // num_t * dt + t0 <= t_final
125            let num_t = ((t_s.last().unwrap() - t0) / dt).floor() as usize;
126            let mut s_t =
127                t_to_s_topp2_core(s, a, t_s, (0..num_t).map(|i| t0 + i as f64 * dt), num_t);
128            if include_final {
129                let flag = if s_t.is_empty() {
130                    t0 <= *t_s.last().unwrap()
131                } else {
132                    *s_t.last().unwrap() < *s.last().unwrap()
133                };
134                if flag {
135                    s_t.push(*s.last().unwrap());
136                }
137            }
138            s_t
139        }
140        InterpolationMode::NonUniformTimeGrid(t_sample) => {
141            if t_sample.is_empty() || t_sample.windows(2).any(|w| w[0] >= w[1]) {
142                // Empty or non-increasing sample sequence is invalid.
143                return vec![];
144            }
145            t_to_s_topp2_core(s, a, t_s, t_sample.iter().cloned(), t_sample.len())
146        }
147    }
148}
149
150/// Core inverse interpolation kernel for `t_to_s_topp2`.
151///
152/// It consumes increasing `t_sample` values and emits corresponding `s(t)` by
153/// segment-wise inversion with quadratic-in-`a` local model.
154fn t_to_s_topp2_core(
155    s: &[f64],
156    a: &[f64],
157    t_s: &[f64],
158    mut t_sample: impl Iterator<Item = f64>,
159    len_t_sample: usize,
160) -> Vec<f64> {
161    let &t_start = t_s.first().unwrap();
162    // Map t to s
163    let mut s_t = Vec::<f64>::with_capacity(len_t_sample + 1); // s_t[i] = s(t[i])
164    let Some(mut t_curr) = t_sample.next() else {
165        return vec![];
166    };
167    while t_curr < t_start {
168        s_t.push(f64::NAN);
169        let Some(t) = t_sample.next() else {
170            return s_t;
171        };
172        t_curr = t;
173    }
174
175    for (s_pair, a_pair, t_pair) in izip!(s.windows(2), a.windows(2), t_s.windows(2)) {
176        while t_curr <= t_pair[1] {
177            s_t.push(
178                s_pair[0]
179                    + inverse_2order(
180                        a_pair[0],
181                        (a_pair[1] - a_pair[0]) / (s_pair[1] - s_pair[0]),
182                        0.0,
183                        t_curr - t_pair[0],
184                    ),
185            );
186            let Some(t) = t_sample.next() else {
187                return s_t;
188            };
189            t_curr = t;
190        }
191    }
192
193    s_t.push(f64::NAN);
194    while t_sample.next().is_some() {
195        s_t.push(f64::NAN);
196    }
197    s_t
198}
199
200/// Solve `x_right` from the integral equation
201/// $dt = \int_{x_{left}}^{x_{right}} \frac{dx}{\sqrt{c_0 + c_1 x}}$.
202#[inline]
203fn inverse_2order(c0: f64, c1: f64, x_left: f64, dt: f64) -> f64 {
204    if dt == 0.0 {
205        x_left
206    } else if c1.abs() > f64::EPSILON {
207        (((c0 + c1 * x_left).sqrt() + 0.5 * c1 * dt).powi(2) - c0) / c1
208    } else if c0.abs() > f64::EPSILON {
209        x_left + c0.sqrt() * dt
210    } else {
211        f64::INFINITY
212    }
213}
copp\copp\copp2/interpolation.rs

copp\copp\copp2/
interpolation.rs
